/** \file IRController.h
 * \brief Useful functions for using the IR beacon.
 *
 * IRController.h contains functions useful for accurately determining the location of the IR beacon.
 */

#include "drivers/hitechnic-sensormux.h"
#include "drivers/hitechnic-irseeker-v2.h"

/**
 * \brief This returns the greater of the two inputs.
 */
#ifndef max
#define max( a, b ) ( ((a) > (b)) ? (a) : (b) )
#endif

/**
 * \brief Returns a more accurate IR beacon location
 *
 * IRGetACDir gets the value of the IR beacon with float precision, rather than integer precision.
 *
 * @param link The sensor multiplexer port location of the IR Seeker V2.
 * @return The direction of the IR beacon.
 */
float IRGetACDir(tMUXSensor link)
{
	float currDir=0;
  static float prevDir = 0.0;

  int acS[5];
  int idx;

  idx = HTIRS2readACDir(link);
  currDir = (float)idx;
  if (idx == 0)
  {
    currDir = prevDir;
  }
  else if (HTIRS2readAllACStrength(link, acS[0], acS[1], acS[2], acS[3], acS[4]))
  {
    idx = (idx - 1)/2;
    if ((idx < 4) && (acS[idx] != 0) && (acS[idx + 1] != 0))
    {
      currDir += (float)(acS[idx + 1] - acS[idx])/
      max(acS[idx], acS[idx + 1]);
    }
    //nxtDisplayTextLine(0, "Idx=%d,Dir=%5.1f", idx, currDir);
    //nxtDisplayTextLine(2, "S1=%d,S2=%d", acS[0], acS[1]);
    //nxtDisplayTextLine(3, "S3=%d,S4=%d", acS[2], acS[3]);
    //nxtDisplayTextLine(4, "S5=%d", acS[4]);
  }
  prevDir = currDir;

  return currDir;
}
